#include "mc_math.h"
#include "stdbool.h"

#define SIN_COS_TABLE {                                             \
    0x0000, 0x00C9, 0x0192, 0x025B, 0x0324, 0x03ED, 0x04B6, 0x057F, \
    0x0648, 0x0711, 0x07D9, 0x08A2, 0x096A, 0x0A33, 0x0AFB, 0x0BC4, \
    0x0C8C, 0x0D54, 0x0E1C, 0x0EE3, 0x0FAB, 0x1072, 0x113A, 0x1201, \
    0x12C8, 0x138F, 0x1455, 0x151C, 0x15E2, 0x16A8, 0x176E, 0x1833, \
    0x18F9, 0x19BE, 0x1A82, 0x1B47, 0x1C0B, 0x1CCF, 0x1D93, 0x1E57, \
    0x1F1A, 0x1FDD, 0x209F, 0x2161, 0x2223, 0x22E5, 0x23A6, 0x2467, \
    0x2528, 0x25E8, 0x26A8, 0x2767, 0x2826, 0x28E5, 0x29A3, 0x2A61, \
    0x2B1F, 0x2BDC, 0x2C99, 0x2D55, 0x2E11, 0x2ECC, 0x2F87, 0x3041, \
    0x30FB, 0x31B5, 0x326E, 0x3326, 0x33DF, 0x3496, 0x354D, 0x3604, \
    0x36BA, 0x376F, 0x3824, 0x38D9, 0x398C, 0x3A40, 0x3AF2, 0x3BA5, \
    0x3C56, 0x3D07, 0x3DB8, 0x3E68, 0x3F17, 0x3FC5, 0x4073, 0x4121, \
    0x41CE, 0x427A, 0x4325, 0x43D0, 0x447A, 0x4524, 0x45CD, 0x4675, \
    0x471C, 0x47C3, 0x4869, 0x490F, 0x49B4, 0x4A58, 0x4AFB, 0x4B9D, \
    0x4C3F, 0x4CE0, 0x4D81, 0x4E20, 0x4EBF, 0x4F5D, 0x4FFB, 0x5097, \
    0x5133, 0x51CE, 0x5268, 0x5302, 0x539B, 0x5432, 0x54C9, 0x5560, \
    0x55F5, 0x568A, 0x571D, 0x57B0, 0x5842, 0x58D3, 0x5964, 0x59F3, \
    0x5A82, 0x5B0F, 0x5B9C, 0x5C28, 0x5CB3, 0x5D3E, 0x5DC7, 0x5E4F, \
    0x5ED7, 0x5F5D, 0x5FE3, 0x6068, 0x60EB, 0x616E, 0x61F0, 0x6271, \
    0x62F1, 0x6370, 0x63EE, 0x646C, 0x64E8, 0x6563, 0x65DD, 0x6656, \
    0x66CF, 0x6746, 0x67BC, 0x6832, 0x68A6, 0x6919, 0x698B, 0x69FD, \
    0x6A6D, 0x6ADC, 0x6B4A, 0x6BB7, 0x6C23, 0x6C8E, 0x6CF8, 0x6D61, \
    0x6DC9, 0x6E30, 0x6E96, 0x6EFB, 0x6F5E, 0x6FC1, 0x7022, 0x7083, \
    0x70E2, 0x7140, 0x719D, 0x71F9, 0x7254, 0x72AE, 0x7307, 0x735E, \
    0x73B5, 0x740A, 0x745F, 0x74B2, 0x7504, 0x7555, 0x75A5, 0x75F3, \
    0x7641, 0x768D, 0x76D8, 0x7722, 0x776B, 0x77B3, 0x77FA, 0x783F, \
    0x7884, 0x78C7, 0x7909, 0x794A, 0x7989, 0x79C8, 0x7A05, 0x7A41, \
    0x7A7C, 0x7AB6, 0x7AEE, 0x7B26, 0x7B5C, 0x7B91, 0x7BC5, 0x7BF8, \
    0x7C29, 0x7C59, 0x7C88, 0x7CB6, 0x7CE3, 0x7D0E, 0x7D39, 0x7D62, \
    0x7D89, 0x7DB0, 0x7DD5, 0x7DFA, 0x7E1D, 0x7E3E, 0x7E5F, 0x7E7E, \
    0x7E9C, 0x7EB9, 0x7ED5, 0x7EEF, 0x7F09, 0x7F21, 0x7F37, 0x7F4D, \
    0x7F61, 0x7F74, 0x7F86, 0x7F97, 0x7FA6, 0x7FB4, 0x7FC1, 0x7FCD, \
    0x7FD8, 0x7FE1, 0x7FE9, 0x7FF0, 0x7FF5, 0x7FF9, 0x7FFD, 0x7FFE}

const int16_t hsin_cos_table[256] = SIN_COS_TABLE;




/**
 * @brief  This function returns cosine and sine functions of the angle fed in
 *         input
 * @param  hangle: 0~32767: 0~pi; -32767~0: pi~2pi
 * @retval Sin(angle) and Cos(angle) in Trig_Components format
 */
TrigComponents trig_functions(int16_t hangle)
{

  int32_t shindex;
  uint16_t uhindex;

  TrigComponents LocalComponents;

  /* 10 bit index computation  */
  shindex = ((int32_t)32768 + (int32_t)hangle);
  uhindex = (uint16_t)shindex;
  uhindex /= (uint16_t)64; // 1024

  switch ((uint16_t)(uhindex)&SIN_MASK)
  {
  case U0_90:
    LocalComponents.hsin = hsin_cos_table[(uint8_t)(uhindex)];
    LocalComponents.hcos = hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    break;

  case U90_180:
    LocalComponents.hsin = hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    LocalComponents.hcos = -hsin_cos_table[(uint8_t)(uhindex)];
    break;

  case U180_270:
    LocalComponents.hsin = -hsin_cos_table[(uint8_t)(uhindex)];
    LocalComponents.hcos = -hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    break;

  case U270_360:
    LocalComponents.hsin = -hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    LocalComponents.hcos = hsin_cos_table[(uint8_t)(uhindex)];
    break;
  default:
    break;
  }
  return (LocalComponents);
}

//Ialpha = id*cos - iq*sin
//Ibeta = id*sin + iq*cos
AlphaBeta_t rev_park_conv(Qd_t input, int16_t theta)
{
  int32_t alpha_tmp1, alpha_tmp2, beta_tmp1, beta_tmp2;
  TrigComponents local_cector_components;
  AlphaBeta_t output;

  local_cector_components = trig_functions(theta);

  /*No overflow guaranteed*/
  alpha_tmp1 = input.q * (int32_t)local_cector_components.hsin;
  alpha_tmp2 = input.d * (int32_t)local_cector_components.hcos;

  output.alpha = (int16_t)((-(alpha_tmp1) + (alpha_tmp2)) >> 15);

  beta_tmp1 = input.q * (int32_t)local_cector_components.hcos;
  beta_tmp2 = input.d * (int32_t)local_cector_components.hsin;

  output.beta = (int16_t)((beta_tmp2 + beta_tmp1) >> 15);
  return (output);
}

//Ialpha =	Iu
//Ibeta  =	1/sqrt(3) * Iu + 2/sqrt(3) * Iv
AlphaBeta_t clarke_conv(Ab_t input)
{
  AlphaBeta_t output;

  int32_t a_divSQRT3_tmp, b_divSQRT3_tmp;
  int32_t wbeta_tmp;
  int16_t hbeta_tmp;

  /* qIalpha = qIas*/
  output.alpha = input.a;

  a_divSQRT3_tmp = DIV_SQRT_3 * (int32_t)input.a;

  b_divSQRT3_tmp = DIV_SQRT_3 * (int32_t)input.b;

  wbeta_tmp = ((a_divSQRT3_tmp) + (b_divSQRT3_tmp) + (b_divSQRT3_tmp)) >> 15;

  /* Check saturation of Ibeta */
  if (wbeta_tmp > INT16_MAX)
  {
    hbeta_tmp = INT16_MAX;
  }
  else if (wbeta_tmp < (-32768))
  {
    hbeta_tmp = (-32768);
  }
  else
  {
    hbeta_tmp = (int16_t)(wbeta_tmp);
  }

  output.beta = hbeta_tmp;

  if (output.beta == (int16_t)(-32768))
  {
    output.beta = -32767;
  }

  return (output);
}

//Id =  Ialpha * cosθ + Ibeta * sinθ
//Iq = -Ialpha * sinθ + Ibeta * cosθ
Qd_t park_conv(AlphaBeta_t input, int16_t theta)
{
  Qd_t output;
  int32_t d_tmp_1, d_tmp_2, q_tmp_1, q_tmp_2;
  TrigComponents local_vector_components;
  int32_t wqd_tmp;
  int16_t hqd_tmp;
  local_vector_components = trig_functions(theta);
  /*No overflow guaranteed*/
  q_tmp_1 = input.alpha * (int32_t)local_vector_components.hsin;
  /*No overflow guaranteed*/
  q_tmp_2 = input.beta * (int32_t)local_vector_components.hcos;
  /*Iq component in Q1.15 Format */
  wqd_tmp = (-q_tmp_1 + q_tmp_2) >> 15; 
  /* Check saturation of Iq */
  if (wqd_tmp > INT16_MAX)
  {
    hqd_tmp = INT16_MAX;
  }
  else if (wqd_tmp < (-32768))
  {
    hqd_tmp = (-32768);
  }
  else
  {
    hqd_tmp = (int16_t)(wqd_tmp);
  }

  output.q = hqd_tmp;

  if (output.q == (int16_t)(-32768))
  {
    output.q = -32767;
  }

  /*No overflow guaranteed*/
  d_tmp_1 = input.alpha * (int32_t)local_vector_components.hcos;

  /*No overflow guaranteed*/
  d_tmp_2 = input.beta * (int32_t)local_vector_components.hsin;


  wqd_tmp = (d_tmp_1 + d_tmp_2) >> 15;


  /* Check saturation of Id */
  if (wqd_tmp > INT16_MAX)
  {
    hqd_tmp = INT16_MAX;
  }
  else if (wqd_tmp < (-32768))
  {
    hqd_tmp = (-32768);
  }
  else
  {
    hqd_tmp = (int16_t)(wqd_tmp);
  }

  output.d = hqd_tmp;

  if (output.d == (int16_t)(-32768))
  {
    output.d = -32767;
  }

  return (output);
}

//Iu = Ia
//Iv = -Ia/2 + sqrt(3) * Ib / 2
//Iw = -Ia/2 - sqrt(3) * Ib / 2
void rev_clarke_conv(AlphaBeta_t input, Uvw_t *uvw)
{
  uvw->u =   input.alpha;
  uvw->v = (-input.alpha / 2) + ((SQRT_3_DIV2 * input.beta) >> 15);
  uvw->w = (-input.alpha / 2) - ((SQRT_3_DIV2 * input.beta) >> 15);
}







